MODULE Module1
    CONST robtarget grip:=[[1063.639018582,-0.563655026,439.67026911],[0.076324061,0.612358407,0.786754088,0.014485282],[0,3,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget safe:=[[867.04338576,-893.440090484,723.024520662],[0.076324137,0.612358651,0.786753894,0.014485128],[-1,4,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget home:=[[1047.898035521,-10.383839117,858.981631414],[0.076324055,0.612358274,0.786754194,0.014485237],[-1,3,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget drop:=[[222.344677643,-772.457436166,166.260750048],[0.076324053,0.612358151,0.786754291,0.014485148],[-1,4,-5,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    !***********************************************************
    !
    ! Module:  Module1
    !
    ! Description:
    !   <Insert description here>
    !
    ! Author: Mechrevo
    !
    ! Version: 1.0
    !
    !***********************************************************
    
    
    !***********************************************************
    !
    ! Procedure main
    !
    !   This is the entry point of your program
    !
    !***********************************************************
    PROC main()
     MoveJ home,v1000,fine,Servo\WObj:=wobj0; !goback home
     FOR i FROM 1 TO 3 DO
         FOR j FROM 1 TO 5 DO
             SetDO DoStartTran,1;  !STELL start move
                 WHILE diPosOK=0 DO   !wait posOK
                     ENDWHILE
                     SetDO DoStartTran,0;
                     MoveL grip,v1000,fine,Servo\WObj:=wobj0; !move grip point
                     SetDO DoStartGrip,1; !start Grip
                     WHILE DiObjOK=0 DO
                     ENDWHILE
                     waittime 0.2;
                    MoveL safe,v1000,fine,Servo\WObj:=wobj0;
                     MoveL drop,v1000,fine,Servo\WObj:=wobj0;
                     SetDO DoStartGrip,0;
                      MoveL safe,v1000,fine,Servo\WObj:=wobj0;
                  MoveL home,v1000,fine,Servo\WObj:=wobj0;       
          
         ENDFOR
         
     ENDFOR

; 
    ENDPROC
    PROC Path_10()
        MoveL grip,v1000,z100,Servo\WObj:=wobj0;
        MoveL drop,v1000,z100,Servo\WObj:=wobj0;
        MoveL safe,v1000,z100,Servo\WObj:=wobj0;
        MoveL home,v1000,z100,Servo\WObj:=wobj0;
    ENDPROC
ENDMODULE